/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright, Nick Johnson
*
*********************************************************************/

// ================================================================== includes

#include <iostream>

using namespace std;

#include <osl_core/ModuleCore.h>
#include <osl_core/LocalCoordConverter.h>

#include <auv_pilot/pilot.h>

// =================================================================== defines

// =================================================================== methods

namespace auv_pilot
{

// ---------------------------------------------------------------------------
Pilot::Pilot()
{
	// Cannot set timers here because ROS is not yet initialised, so do all
	// setup in init().
	initialised_ = false;
}

// ---------------------------------------------------------------------------
Pilot::~Pilot()
{

}

// ---------------------------------------------------------------------------
bool Pilot::init(ros::NodeHandlePtr rosHandle)
{
	if (!conf_.initROSParam(rosHandle, "conf/autopilot/pid"))
	{
		// Can't do anything without configuration, so return.
		return false;
	}

	if (!readPIDParams()) {
		PRINTLN_ERR("Failed to read PID gains - aborting.");
		return false;
	}

        // Time to timeout old waypoint and allow new one with the same priority.
        staleWaypointTimeout_ = 500;
        // Waypoint will be immediately stale after reset.
        staleWaypointTimer_.setTimer(1);

        currentPose_.reset();
        currentVel_.reset();

        currentWaypoint_.reset();

        requestTol_.reset();
        axisEnabled_.reset();
	axisEnabledVel_.reset();

        requestPose_.reset();
        requestVel_.reset();

    	receivedWorldPose_ = false;

	resetControl();

	minAltitude_ = 0.7; // DVL limit to work

	initialised_ = true;

	return true;
}

// ---------------------------------------------------------------------------
void Pilot::resetControl()
{
	poseError_.reset();

	posePID_.x.reset();
	posePID_.y.reset();
	posePID_.z.reset();
	posePID_.roll.reset();
	posePID_.pitch.reset();
	posePID_.yaw.reset();

	velError_.reset();

	velPID_.x.reset();
	velPID_.y.reset();
	velPID_.z.reset();
	velPID_.roll.reset();
	velPID_.pitch.reset();
	velPID_.yaw.reset();

	requestForce_.reset();
}

// ---------------------------------------------------------------------------
void Pilot::resetControlVelocity()
{
	
	velError_.reset();

	velPID_.x.reset();
	velPID_.y.reset();
	velPID_.z.reset();
	velPID_.roll.reset();
	velPID_.pitch.reset();
	velPID_.yaw.reset();
	axisEnabledVel_.reset();
	requestVelMode_.reset();
	requestForce_.reset();
}

// ---------------------------------------------------------------------------
bool Pilot::readPIDParams()
{
	const char *sectionName[] = { "AUTOPOS_X_POS", "AUTOPOS_Y_POS", "AUTOPOS_Z_POS", "AUTOPOS_PITCH_POS", "AUTOPOS_YAW_POS",
			"AUTOPOS_X_VEL", "AUTOPOS_Y_VEL", "AUTOPOS_Z_VEL", "AUTOPOS_PITCH_VEL", "AUTOPOS_YAW_VEL", NULL
	};

	Pid *pid[] = { &posePID_.x, &posePID_.y, &posePID_.z, &posePID_.pitch, &posePID_.yaw,
			&velPID_.x, &velPID_.y, &velPID_.z, &velPID_.pitch, &velPID_.yaw
	};

	int pidNum = 0;
	while (sectionName[pidNum] != NULL)
	{
		float pGain, iGain, dGain, /*inputLimit,*/ integralLimit;

		if (!conf_.getFloatCached(pGain, "pGain", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(iGain, "iGain", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(dGain, "dGain", sectionName[pidNum])) return false;
		//if (!conf_.getFloat(inputLimit, "inputLimit", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(integralLimit, "integralLimit", sectionName[pidNum])) return false;

		pid[pidNum]->initPid(pGain, iGain, dGain, integralLimit, -integralLimit);

		pidNum++;
	}

	// TODO: Move vehicle velocity limit parameters out of the PID parameter block.
	// We don't have position error limits any more.
	if (!conf_.getFloatCached(velLimit_.x, "inputLimit", sectionName[5])) return false;
	if (!conf_.getFloatCached(velLimit_.y, "inputLimit", sectionName[6])) return false;
	if (!conf_.getFloatCached(velLimit_.z, "inputLimit", sectionName[7])) return false;
	if (!conf_.getFloatCached(velLimit_.pitch, "inputLimit", sectionName[8])) return false;
	if (!conf_.getFloatCached(velLimit_.yaw, "inputLimit", sectionName[9])) return false;
	
	return true;
}
// ---------------------------------------------------------------------------
bool Pilot::updatePIDParams()
{
	const char *sectionName[] = { "AUTOPOS_X_POS", "AUTOPOS_Y_POS", "AUTOPOS_Z_POS", "AUTOPOS_PITCH_POS", "AUTOPOS_YAW_POS",
			"AUTOPOS_X_VEL", "AUTOPOS_Y_VEL", "AUTOPOS_Z_VEL", "AUTOPOS_PITCH_VEL", "AUTOPOS_YAW_VEL", NULL
	};

	Pid *pid[] = { &posePID_.x, &posePID_.y, &posePID_.z, &posePID_.pitch, &posePID_.yaw,
			&velPID_.x, &velPID_.y, &velPID_.z, &velPID_.pitch, &velPID_.yaw
	};

	int pidNum = 0;
	while (sectionName[pidNum] != NULL)
	{
		float pGain, iGain, dGain, /*inputLimit,*/ integralLimit;

		if (!conf_.getFloatCached(pGain, "pGain", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(iGain, "iGain", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(dGain, "dGain", sectionName[pidNum])) return false;
		//if (!conf_.getFloat(inputLimit, "inputLimit", sectionName[pidNum])) return false;
		if (!conf_.getFloatCached(integralLimit, "integralLimit", sectionName[pidNum])) return false;

		double oldpGain, oldiGain, olddGain, oldMaxIntegralLimit, oldMinIntegralLimit;
		pid[pidNum]->getGains(oldpGain, oldiGain, olddGain, oldMaxIntegralLimit, oldMinIntegralLimit);

		//If any of the values has been updated we change the PID
		if(oldpGain != pGain || oldiGain != iGain || olddGain != dGain || integralLimit != oldMaxIntegralLimit){
			pid[pidNum]->initPid(pGain, iGain, dGain, integralLimit, -integralLimit);
		}
		pidNum++;

	}

	// TODO: Move vehicle velocity limit parameters out of the PID parameter block.
	// We don't have position error limits any more.
	if (!conf_.getFloatCached(velLimit_.x, "inputLimit", sectionName[5])) return false;
	if (!conf_.getFloatCached(velLimit_.y, "inputLimit", sectionName[6])) return false;
	if (!conf_.getFloatCached(velLimit_.z, "inputLimit", sectionName[7])) return false;
	if (!conf_.getFloatCached(velLimit_.pitch, "inputLimit", sectionName[8])) return false;
	if (!conf_.getFloatCached(velLimit_.yaw, "inputLimit", sectionName[9])) return false;
	
	return true;
}

// ---------------------------------------------------------------------------
bool Pilot::writePIDParams(){

	const char *sectionName[] = { "AUTOPOS_X_POS", "AUTOPOS_Y_POS", "AUTOPOS_Z_POS", "AUTOPOS_PITCH_POS", "AUTOPOS_YAW_POS",
			"AUTOPOS_X_VEL", "AUTOPOS_Y_VEL", "AUTOPOS_Z_VEL", "AUTOPOS_PITCH_VEL", "AUTOPOS_YAW_VEL", NULL
	};

	Pid *pid[] = { &posePID_.x, &posePID_.y, &posePID_.z, &posePID_.pitch, &posePID_.yaw,
			&velPID_.x, &velPID_.y, &velPID_.z, &velPID_.pitch, &velPID_.yaw
	};


	int pidNum = 0;
	while (sectionName[pidNum] != NULL)
	{
		double pGain, iGain, dGain, /*inputLimit,*/ integralLimit, minIntegralLimit;

		pid[pidNum]->getGains(pGain, iGain, dGain, integralLimit, minIntegralLimit);

		conf_.setFloat(pGain, "pGain", sectionName[pidNum]);
		conf_.setFloat(iGain, "iGain", sectionName[pidNum]);
		conf_.setFloat(dGain, "dGain", sectionName[pidNum]);
		//conf_.setFloat(inputLimit, "inputLimit", sectionName[pidNum]);
		conf_.setFloat(integralLimit, "integralLimit", sectionName[pidNum]);

		pidNum++;
	}


	// TODO: Move vehicle velocity limit parameters out of the PID parameter block.
	// We don't have position error limits any more.
	conf_.setFloat(velLimit_.x, "inputLimit", sectionName[5]);
	conf_.setFloat(velLimit_.y, "inputLimit", sectionName[6]);
	conf_.setFloat(velLimit_.z, "inputLimit", sectionName[7]);
	conf_.setFloat(velLimit_.pitch, "inputLimit", sectionName[8]);
	conf_.setFloat(velLimit_.yaw, "inputLimit", sectionName[9]);

	if (!conf_.save()) {
		PRINTLN_ERR("Failed to save PID parameters.");
	} else {
		PRINTLN_INFO("Saved PID parameters.");
	}

	return true;
}

// ---------------------------------------------------------------------------
void Pilot::printDiag()
{
	PRINTLN_INFO("--------------------------------------------------------");
	PRINTLN_INFO("VARIABLE     REQUEST     CURRENT       ERROR   TOLERANCE");
	PRINTLN_INFO("--------------------------------------------------------");

	// Float print width.
	int w = 9;
	// Gap between columns.
	const std::string gap = "   ";
	// Blank string same size as w.
	const std::string blank = "         ";
	const std::string gap_blank = gap + blank;
	const std::string gap_disabled = gap + "      dis";
	// Float print precision (decimal places).
	int p = 3;

	// Print NEDA world frame request and current pose, with errors but not tolerances.
	// Tolerances apply to body frame.
	if (axisEnabled_.x || axisEnabled_.y) {
		PRINTLN_INFO("North : "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.n
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.n
				<< gap << fixed << setw(w) << setprecision(p) << (currentPose_.n - requestPose_.n));
	}
	else {
		PRINTLN_INFO("North : "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.n
				<< gap_disabled);
	}

	if (axisEnabled_.x || axisEnabled_.y) {
		PRINTLN_INFO("East :  "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.e
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.e
				<< gap << fixed << setw(w) << setprecision(p) << (currentPose_.e - requestPose_.e));
	}
	else {
		PRINTLN_INFO("East :  "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.e
				<< gap_disabled);
	}

	if (axisEnabled_.z && requestDepthMode_) {
		PRINTLN_INFO("Depth : "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.d
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.d
				<< gap << fixed << setw(w) << setprecision(p) << (currentPose_.d - requestPose_.d));
	}
	else {
		PRINTLN_INFO("Depth : "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.d
				<< gap_disabled);
	}

	if (axisEnabled_.z && !requestDepthMode_) {
		PRINTLN_INFO("Alti :  "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.a
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.a
				<< gap << fixed << setw(w) << setprecision(p) << (currentPose_.a - requestPose_.a));
	}
	else {
		PRINTLN_INFO("Alti :  "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.a
				<< gap_disabled);
	}

		// Now print body frame position errors and tolerances.
	if (axisEnabled_.x) {
		PRINTLN_INFO("X :     "
				<< gap_blank
				<< gap_blank
				<< gap << fixed << setw(w) << setprecision(p) << poseError_.x
				<< gap << fixed << setw(w) << setprecision(p) << requestTol_.x);
	} else {
		PRINTLN_INFO("X :     "
				<< gap_blank
				<< gap_blank
				<< gap_disabled
				<< gap_disabled);
	}

	if (axisEnabled_.y) {
		PRINTLN_INFO("Y :     "
				<< gap_blank
				<< gap_blank
				<< gap << fixed << setw(w) << setprecision(p) << poseError_.y
				<< gap << fixed << setw(w) << setprecision(p) << requestTol_.y);
	} else {
		PRINTLN_INFO("Y :     "
				<< gap_blank
				<< gap_blank
				<< gap_disabled
				<< gap_disabled);
	}

	if (axisEnabled_.z) {
		PRINTLN_INFO("Z :     "
				<< gap_blank
				<< gap_blank
				<< "   " << fixed << setw(w) << setprecision(p) << poseError_.z
				<< "   " << fixed << setw(w) << setprecision(p) << requestTol_.z);
	} else {
		PRINTLN_INFO("Z :     "
				<< gap_blank
				<< gap_blank
				<< gap_disabled
				<< gap_disabled);
	}

	// Now print poses, which are fixed angles so the same in world and body frame.

	if (axisEnabled_.pitch) {
		PRINTLN_INFO("Pitch : "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.pitch
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.pitch
				<< gap << fixed << setw(w) << setprecision(p) << poseError_.pitch
				<< gap << fixed << setw(w) << setprecision(p) << requestTol_.pitch);
	} else {
		PRINTLN_INFO("Pitch : "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.pitch
				<< gap_disabled
				<< gap_disabled);
	}

	if (axisEnabled_.yaw) {
		PRINTLN_INFO("Yaw :   "
				<< gap << fixed << setw(w) << setprecision(p) << requestPose_.yaw
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.yaw
				<< gap << fixed << setw(w) << setprecision(p) << poseError_.yaw
				<< gap << fixed << setw(w) << setprecision(p) << requestTol_.yaw);
	} else {
		PRINTLN_INFO("Yaw :   "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentPose_.yaw
				<< gap_disabled
				<< gap_disabled);
	}

	PRINTLN_INFO("--------------------------------------------------------");

	// Now print body frame velocities.
	if (axisEnabled_.x) {
		PRINTLN_INFO("X' :    "
				<< gap << fixed << setw(w) << setprecision(p) << requestVel_.x
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.x
				<< gap << fixed << setw(w) << setprecision(p) << velError_.x
				<< gap_blank);
	} else {
		PRINTLN_INFO("X' :    "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.x
				<< gap_disabled);
	}

	if (axisEnabled_.y) {
		PRINTLN_INFO("Y' :    "
				<< gap << fixed << setw(w) << setprecision(p) << requestVel_.y
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.y
				<< gap << fixed << setw(w) << setprecision(p) << velError_.y);
	} else {
		PRINTLN_INFO("Y' :    "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.y
				<< gap_disabled);
	}

	if (axisEnabled_.z) {
		PRINTLN_INFO("Z' :    "
				<< gap << fixed << setw(w) << setprecision(p) << requestVel_.z
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.z
				<< gap << fixed << setw(w) << setprecision(p) << velError_.z);
	} else {
		PRINTLN_INFO("Z' :    "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.z
				<< gap_disabled);
	}

	if (axisEnabled_.pitch) {
		PRINTLN_INFO("Pitch' :"
				<< gap << fixed << setw(w) << setprecision(p) << requestVel_.pitch
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.pitch
				<< gap << fixed << setw(w) << setprecision(p) << velError_.pitch);
	} else {
		PRINTLN_INFO("Pitch' :"
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.pitch
				<< gap_disabled);
	}

	if (axisEnabled_.yaw) {
		PRINTLN_INFO("Yaw' :  "
				<< gap << fixed << setw(w) << setprecision(p) << requestVel_.yaw
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.yaw
				<< gap << fixed << setw(w) << setprecision(p) << velError_.yaw);
	} else {
		PRINTLN_INFO("Yaw' :  "
				<< gap_disabled
				<< gap << fixed << setw(w) << setprecision(p) << currentVel_.yaw
				<< gap_disabled);
	}

	PRINTLN_INFO("--------------------------------------------------------");
	PRINTLN_INFO("Waypoint: " << currentWaypoint_.toString());
	PRINTLN_INFO("Axis forces: x " << fixed << setw(5) << setprecision(1) << requestForce_.x
			<< ", y " << setw(5) << setprecision(1) << requestForce_.y
			<< ", z " << setw(5) << setprecision(1) << requestForce_.z
			<< ", pitch " << setw(5) << setprecision(1) << requestForce_.pitch
			<< ", yaw " << setw(5) << setprecision(1) << requestForce_.yaw);

	//Show PID params on screen.
#warning

	double  pose_pe_x,pose_ie_x, pose_de_x ;
	double  vel_pe_x, vel_ie_x, vel_de_x;

	double  pose_pe_y, pose_ie_y, pose_de_y;
	double  vel_pe_y, vel_ie_y, vel_de_y;

	double  pose_pe_z, pose_ie_z, pose_de_z;
	double  vel_pe_z, vel_ie_z, vel_de_z;

	double  pose_pe_pitch, pose_ie_pitch, pose_de_pitch;
	double  vel_pe_pitch, vel_ie_pitch, vel_de_pitch;

	double pose_pe_yaw,pose_ie_yaw,pose_de_yaw;
	double vel_pe_yaw,vel_ie_yaw,vel_de_yaw;


	pose_pe_x = pose_ie_x = pose_de_x = 0;
	vel_pe_x = vel_ie_x = vel_de_x= 0;
	pose_pe_y = pose_ie_y = pose_de_y= 0;
	vel_pe_y = vel_ie_y = vel_de_y= 0;
	pose_pe_z = pose_ie_z= pose_de_z= 0;
	vel_pe_z = vel_ie_z= vel_de_z= 0;
	pose_pe_pitch = pose_ie_pitch= pose_de_pitch= 0;
	vel_pe_pitch= vel_ie_pitch= vel_de_pitch= 0;
	pose_pe_yaw= pose_ie_yaw= pose_de_yaw= 0;
	vel_pe_yaw= vel_ie_yaw= vel_de_yaw= 0;

	posePID_.x.getCurrentPIDErrors(&pose_pe_x, &pose_ie_x, &pose_de_x);
	velPID_.x.getCurrentPIDErrors(&vel_pe_x, &vel_ie_x, &vel_de_x);

	posePID_.y.getCurrentPIDErrors(&pose_pe_y, &pose_ie_y, &pose_de_y);
	velPID_.y.getCurrentPIDErrors(&vel_pe_y, &vel_ie_y, &vel_de_y);

	posePID_.z.getCurrentPIDErrors(&pose_pe_z, &pose_ie_z, &pose_de_z);
	velPID_.z.getCurrentPIDErrors(&vel_pe_z, &vel_ie_z, &vel_de_z);

	posePID_.pitch.getCurrentPIDErrors(&pose_pe_pitch, &pose_ie_pitch, &pose_de_pitch);
	velPID_.pitch.getCurrentPIDErrors(&vel_pe_pitch, &vel_ie_pitch, &vel_de_pitch);

	posePID_.yaw.getCurrentPIDErrors(&pose_pe_yaw, &pose_ie_yaw, &pose_de_yaw);
	velPID_.yaw.getCurrentPIDErrors(&vel_pe_yaw, &vel_ie_yaw, &vel_de_yaw);

	ROS_INFO_STREAM("X POSE ERRORS: Proportional:"<< pose_pe_x << ", Integral: " << pose_ie_x << ", Derivative: " << pose_de_x); 
	ROS_INFO_STREAM("X VEL ERRORS: Proportional:"<< vel_pe_x << ", Integral: " << vel_ie_x << ", Derivative: " << vel_de_x); 
	ROS_INFO_STREAM("Y POSE ERRORS: Proportional:"<< pose_pe_y << ", Integral: " << pose_ie_y << ", Derivative: " << pose_de_y); 
	ROS_INFO_STREAM("Y VEL ERRORS: Proportional:"<< vel_pe_y << ", Integral: " << vel_ie_y << ", Derivative: " << vel_de_y); 
	ROS_INFO_STREAM("Z POSE ERRORS: Proportional:"<< pose_pe_z << ", Integral: " << pose_ie_z << ", Derivative: " << pose_de_z); 
	ROS_INFO_STREAM("Z VEL ERRORS: Proportional:"<< vel_pe_z << ", Integral: " << vel_ie_z << ", Derivative: " << pose_de_z); 
	ROS_INFO_STREAM("YAW POSE ERRORS: Proportional:"<< pose_pe_yaw << ", Integral: " << pose_ie_yaw << ", Derivative: " << pose_de_yaw); 
	ROS_INFO_STREAM("YAW VEL ERRORS: Proportional:"<< vel_pe_yaw << ", Integral: " << vel_ie_yaw << ", Derivative: " << vel_de_yaw); 
	ROS_INFO_STREAM("PITCH POSE ERRORS: Proportional:"<< pose_pe_pitch << ", Integral: " << pose_ie_pitch << ", Derivative: " << pose_de_pitch); 
	ROS_INFO_STREAM("PITCH VEL ERRORS: Proportional:"<< vel_pe_pitch << ", Integral: " << vel_ie_pitch << ", Derivative: " << vel_de_pitch); 




}

// ---------------------------------------------------------------------------
void Pilot::setWorldPose(double north, double east, double depth, double altitude, double roll, double pitch, double yaw)
{
	currentPose_.n = north;
	currentPose_.e = east;
	currentPose_.d = depth;
	currentPose_.a = altitude;
	currentPose_.roll = roll;
	currentPose_.pitch = pitch;
	currentPose_.yaw = yaw;

	receivedWorldPose_ = true;
}

// ---------------------------------------------------------------------------
void Pilot::setBodyVelocity(double x, double y, double z, double roll, double pitch, double yaw)
{
	currentVel_.x = x;
	currentVel_.y = y;
	currentVel_.z = z;
	currentVel_.roll = roll;
	currentVel_.pitch = pitch;
	currentVel_.yaw = yaw;
}

// ---------------------------------------------------------------------------
bool Pilot::checkAndSetNewWaypoint(const std::string& requester, uint32_t goalID, uint32_t priority)
{
	bool useNew = false;

	if (priority > currentWaypoint_.priority) {
		PRINTLN_INFO("Previous waypoint being overridden: " << currentWaypoint_.toString());
		useNew = true;
	}
	else if (staleWaypointTimer_.getTimer() == 0) {
		PRINTLN_INFO("Waypoint stale timer had expired for waypoint: " << currentWaypoint_.toString());
		useNew = true;
	}
	else {
		//PRINTLN_INFO("Ignoring lower or same priority waypoint: " << requester << ", id " << goalID);
	}

	if (useNew) {
		currentWaypoint_.set(requester, goalID, priority);
		PRINTLN_INFO("New waypoint: " << currentWaypoint_.toString());
		setStaleWaypointTimer();
	}

	return useNew;
}

// ---------------------------------------------------------------------------
void Pilot::setStaleWaypointTimer()
{
	staleWaypointTimer_.setTimer(staleWaypointTimeout_);
}

// ---------------------------------------------------------------------------
bool Pilot::requestWorldWaypoint(const std::string& requester, uint32_t goalID, uint32_t priority,
		double north, double east, double depth, double altitude, bool depthMode,
		double pitch, double yaw,
		double xTol, double yTol, double zTol, double pitchTol, double yawTol,
		bool xEnable, bool yEnable, bool zEnable, bool pitchEnable, bool yawEnable)
{
	if (!receivedWorldPose_) {
		PRINTLN_WARN("Waypoint issued before vehicle pose received - ignoring.");
		return false;
	}

	if (requester == currentWaypoint_.requester && goalID == currentWaypoint_.id) {
		// Ignore repeated waypoint - we are already dealing with it.
		setStaleWaypointTimer();
		return true;
	}

	// Check waypoint priorities.
	if (!checkAndSetNewWaypoint(requester, goalID, priority)) {
		return false;
	}

	// At this point we are going to apply the waypoint, so reset the PID
	// controllers first, to avoid using stale integral.
	resetControl();

	requestPose_.set(north, east, depth, altitude, 0, pitch, yaw);
	requestDepthMode_ = depthMode;

	// Zero depth/altitude when unused, for clarity.
	if (requestDepthMode_) {
		requestPose_.a = 0;
	}
	else {
		requestPose_.d = 0;
	}

	requestTol_.set(xTol, yTol, zTol, 0, pitchTol, yawTol);
	axisEnabled_.set(xEnable, yEnable, zEnable, false, pitchEnable, yawEnable);

	PRINTLN_INFO("requestWorldWaypoint: " << " n " << requestPose_.n << ", e " << requestPose_.e
			<< (requestDepthMode_ ? ", d " : ", a ") << (requestDepthMode_ ? requestPose_.d : requestPose_.a)
			<< ", pitch " << requestPose_.pitch << ", yaw " << requestPose_.yaw);

	// Do a first update of the PIDs, with zero time delta.
	update(0);

	return true;
}

// ---------------------------------------------------------------------------
bool Pilot::requestBodyWaypoint(const std::string& requester, uint32_t goalID, uint32_t priority,
		double x, double y, double z, bool depthMode, double pitch, double yaw,
		double xTol, double yTol, double zTol, double pitchTol, double yawTol,
		bool xEnable, bool yEnable, bool zEnable, bool pitchEnable, bool yawEnable)
{
	if (!receivedWorldPose_) {
		PRINTLN_WARN("Waypoint issued before vehicle pose received - ignoring.");
		return false;
	}

	if (requester == currentWaypoint_.requester && goalID == currentWaypoint_.id) {
		// Ignore repeated waypoint - we are already dealing with it.
		setStaleWaypointTimer();
		return true;
	}

	// Check waypoint priorities.
	if (!checkAndSetNewWaypoint(requester, goalID, priority)) {
		return false;
	}

        // At this point we are going to apply the waypoint, so reset the PID
        // controllers first, to avoid using stale integral.
        resetControl();

	osl_core::LocalCoordConverter localConv;
	osl_core::T_NED currentNED;
	osl_core::T_RPY currentRPY;
	osl_core::T_VCS offsetVCS;

	requestDepthMode_ = depthMode;

	// Set current (start) world position.
	currentNED.n = currentPose_.n;
	currentNED.e = currentPose_.e;
	// Use either depth or negative altitude for this calculation, depending on depth mode.
	if (requestDepthMode_) currentNED.d = currentPose_.d;
	else currentNED.d = -currentPose_.a;

	// Set current (start) orientation.
	currentRPY.r = 0;
	currentRPY.p = currentPose_.pitch;
	currentRPY.y = currentPose_.yaw;

	// Set position offset
	offsetVCS.u = x;
	offsetVCS.v = y;
	offsetVCS.w = z;

	// Initialise converter with current position and orientation of vehicle as origin.
	localConv.setReferenceDeg(currentNED, currentRPY);
	// Map requested position to vehicle coordinate frame using that origin.
	osl_core::T_NED requestNED = localConv.VCS2NED(offsetVCS);

	// Now set request from the calculated NED.
	requestPose_.n = requestNED.n;
	requestPose_.e = requestNED.e;
	if (requestDepthMode_) {
		requestPose_.d = requestNED.d;
		requestPose_.a = 0;
	}
	else {
		requestPose_.d = 0;
		// Altitude is just inverted depth from the calculation.
		requestPose_.a = -requestNED.d;
	}

	// TODO pitch control in relative mode? As we're controlling relative to current pose,
	// the effective request could creep over time.
	requestPose_.pitch = osl_core::limitPlusMinus(currentPose_.pitch + pitch, 45);

	requestPose_.yaw = osl_core::wrapPlusMinus180(currentPose_.yaw + yaw);

	requestTol_.set(xTol, yTol, zTol, 0, pitchTol, yawTol);
	axisEnabled_.set(xEnable, yEnable, zEnable, false, pitchEnable, yawEnable);

	PRINTLN_INFO("requestBodyWaypoint: relative " << "x " << x << ", y " << y << ", z " << z
			<< ", pitch " << requestPose_.pitch << ", yaw " << requestPose_.yaw);

	PRINTLN_INFO("requestWorldWaypoint: current " << "n " << currentPose_.n << ", e " << currentPose_.e
			<< (requestDepthMode_ ? ", d " : ", a ") << (requestDepthMode_ ? currentPose_.d : currentPose_.a)
			<< ", pitch " << currentPose_.pitch << ", yaw " << currentPose_.yaw);

	PRINTLN_INFO("requestWorldWaypoint: request " << "n " << requestPose_.n << ", e " << requestPose_.e
			<< (requestDepthMode_ ? ", d " : ", a ") << (requestDepthMode_ ? requestPose_.d : requestPose_.a)
			<< ", pitch " << requestPose_.pitch << ", yaw " << requestPose_.yaw);

	// Do a first update of the PIDs, with zero time delta.
	update(0);

	return true;
}

// ---------------------------------------------------------------------------
bool Pilot::requestStayWaypoint(const std::string& requester, uint32_t goalID, uint32_t priority, bool depthMode,
		double xTol, double yTol, double zTol, double pitchTol, double yawTol,
		bool xEnable, bool yEnable, bool zEnable, bool pitchEnable, bool yawEnable)
{
	if (!receivedWorldPose_) {
		PRINTLN_WARN("Waypoint issued before vehicle pose received - ignoring.");
		return false;
	}

	if (requester == currentWaypoint_.requester && goalID == currentWaypoint_.id) {
		// Ignore repeated waypoint - we are already dealing with it.
		setStaleWaypointTimer();
		return true;
	}

	// Check waypoint priorities.
	if (!checkAndSetNewWaypoint(requester, goalID, priority)) {
		return false;
	}

        // At this point we are going to apply the waypoint, so reset the PID
        // controllers first, to avoid using stale integral.
        resetControl();

	requestPose_.set(currentPose_.n, currentPose_.e, currentPose_.d, currentPose_.a,
			0, currentPose_.pitch, currentPose_.yaw);
	requestDepthMode_ = depthMode;

	// Zero depth/altitude when unused, for clarity.
	if (requestDepthMode_) {
		requestPose_.a = 0;
	}
	else {
		requestPose_.d = 0;
	}

	requestTol_.set(xTol, yTol, zTol, 0, pitchTol, yawTol);
	axisEnabled_.set(xEnable, yEnable, zEnable, 0, pitchEnable, yawEnable);

	PRINTLN_INFO("requestStayWaypoint: " << " n " << requestPose_.n << ", e " << requestPose_.e
			<< (requestDepthMode_ ? ", d " : ", a ") << (requestDepthMode_ ? requestPose_.d : requestPose_.a)
			<< ", pitch " << requestPose_.pitch << ", yaw " << requestPose_.yaw);

	// Do a first update of the PIDs, with zero time delta.
	update(0);

	return true;
}

// ---------------------------------------------------------------------------
void Pilot::manualForceRequest(double x, double y, double z, double pitch, double yaw)
{
	// Set forces, clamping +/- 100.
	requestForce_.x = osl_core::limitPlusMinus(x, 100);
	requestForce_.y = osl_core::limitPlusMinus(y, 100);
	requestForce_.z = osl_core::limitPlusMinus(z, 100);
	requestForce_.roll = 0;
	requestForce_.pitch = osl_core::limitPlusMinus(pitch, 100);
	requestForce_.yaw = osl_core::limitPlusMinus(yaw, 100);
}

// ---------------------------------------------------------------------------
void Pilot::velocityRequest(double x, double y, double z, double pitch, double yaw,
	bool xEnable, bool yEnable, bool zEnable, bool pitchEnable, bool yawEnable)
{
        // Reset the PID controllers first, to avoid using stale integral.
	// This makes the velocity PID's integral component reset each time
	// the velocity request comes in, which isn't ideal, but we don't
        // have a waypoint ID to tell us if it is a new request.
       	resetControlVelocity(); // Not done as probably not necessary


	axisEnabledVel_.set(xEnable, yEnable, zEnable, 0, pitchEnable, yawEnable);

	
	// Set request for velocity in the 5 axes we can control, x, y, z, pitch and yaw if enabled
	if (axisEnabledVel_.x) requestVelMode_.x = osl_core::limitPlusMinus(x, velLimit_.x);
	if (axisEnabledVel_.y) requestVelMode_.y = osl_core::limitPlusMinus(y, velLimit_.y);
	if (axisEnabledVel_.z) requestVelMode_.z = osl_core::limitPlusMinus(z, velLimit_.z);
	if (axisEnabledVel_.pitch) requestVelMode_.pitch = osl_core::limitPlusMinus(pitch, velLimit_.pitch);
	if (axisEnabledVel_.yaw) requestVelMode_.yaw =osl_core::limitPlusMinus(yaw, velLimit_.yaw);
}


void Pilot::update(double deltaSec)
{
	if (!initialised_) {
		PRINTLN_ERR("Pilot::update called before init!");
		return;
	}

	// Calculating error in vehicle frame x, y and z using local coordinate converter.
	osl_core::LocalCoordConverter localConv;
	osl_core::T_NED currentNED, requestNED;
	osl_core::T_RPY currentRPY;

	// Set current (start) world position.
	currentNED.n = currentPose_.n;
	currentNED.e = currentPose_.e;

	// Use either depth or negative altitude for this calculation, depending on depth mode.
	if (requestDepthMode_) currentNED.d = currentPose_.d;
	else currentNED.d = -currentPose_.a;

	// Set request (end) world position.
	requestNED.n = requestPose_.n;
	requestNED.e = requestPose_.e;
	// Use either depth or negative altitude for this calculation, depending on depth mode.
	if (requestDepthMode_) requestNED.d = requestPose_.d;
	else requestNED.d = -requestPose_.a;

	// Set current orientation.
	currentRPY.r = 0;
	currentRPY.p = currentPose_.pitch;
	currentRPY.y = currentPose_.yaw;

	// Initialise converter with current position and orientation of vehicle as origin.
	localConv.setReferenceDeg(currentNED, currentRPY);
	// Map requested position to vehicle coordinate frame using that origin.
	osl_core::T_VCS errorVCS = localConv.NED2VCS(requestNED);

	// Errors need to be inverse of the result, as we mapped from current to requested.
	poseError_.x = -errorVCS.u;
	poseError_.y = -errorVCS.v;
	
	// TODO: Revist z error code that breaks when current/requested position far from origin.
	//poseError_.z = -errorVCS.w;
	// Try to do it in absolute coordinates - YRP 20/04/2012. Seems to work. Problem at high pitch because no vector trust.
	if (requestDepthMode_) poseError_.z = currentPose_.d - requestPose_.d;
	else poseError_.z = requestPose_.a - currentPose_.a;

	//PRINTLN_INFO("poseError_.z: " << poseError_.z << "\tcurrentNED.d: " << currentNED.d  << "\trequestNED.d" << requestNED.d);

	double approxSetPointAltitude = currentPose_.a + poseError_.z;

	// This calculation ignores pitch, but will result in conservative clamping.
	if (approxSetPointAltitude <= minAltitude_) {
		double approxOvershoot = minAltitude_ - approxSetPointAltitude;
//		PRINTLN_INFO("approx z setpoint altitude " << approxSetPointAltitude << " below min " << minAltitude_
//				<< "; clamping z err " << poseError_.z << " to " << (poseError_.z + approxOvershoot));
		poseError_.z += approxOvershoot;
	}

	poseError_.roll = 0;
	// Calculate pitch error with simple subtraction.
	// TODO: checking of pitch wrap? We should only be dealing with small (< 45 deg) pitch...
	poseError_.pitch = currentPose_.pitch - requestPose_.pitch;
	// Calculate yaw error with simple subtraction, followed by wrapping to ensure we
	// turn the quickest way.
	poseError_.yaw = osl_core::wrapPlusMinus180(currentPose_.yaw - requestPose_.yaw);

	// First set force requests to zero, in case axes are disabled.
	requestForce_.reset();

	// Now do the calculations as if all axes were enabled.
	// cout << "requestVel_.pitch:" << requestVel_.pitch << endl;
	// Update the position controllers, clamping to +/- velocity limit.
	// Update all axis if not in velocity mode	
	// Only update the ones that are not enabled in the velocity controller in velocity mode
	
	if(!axisEnabledVel_.x) 
		requestVel_.x = osl_core::limitPlusMinus(posePID_.x.updatePid(poseError_.x, deltaSec), velLimit_.x);
	if(!axisEnabledVel_.y) 
		requestVel_.y = osl_core::limitPlusMinus(posePID_.y.updatePid(poseError_.y, deltaSec), velLimit_.y);
	if(!axisEnabledVel_.z) 
		requestVel_.z = osl_core::limitPlusMinus(posePID_.z.updatePid(poseError_.z, deltaSec), velLimit_.z);
	if(!axisEnabledVel_.pitch) 
		requestVel_.pitch = osl_core::limitPlusMinus(posePID_.pitch.updatePid(poseError_.pitch, deltaSec), velLimit_.pitch);
	if(!axisEnabledVel_.yaw) 
		requestVel_.yaw = osl_core::limitPlusMinus(posePID_.yaw.updatePid(poseError_.yaw, deltaSec), velLimit_.yaw);
	// Check altitude is not too low. Cut speed to zeros in the z axis if this is the case.
	if (currentPose_.a <= minAltitude_ && requestVel_.z > 0)
	{
	//	PRINTLN_INFO("clamping positive z velocity req " << requestVel_.z << " to zero when below min altitude.");
		requestVel_.z = 0.0;
	}
	// cout << "requestVel_.pitch:" << requestVel_.pitch << endl;
	// Calculate velocity errors.
	velError_.x = currentVel_.x - requestVel_.x;
	velError_.y = currentVel_.y - requestVel_.y;
	velError_.z = currentVel_.z - requestVel_.z;
	velError_.pitch = currentVel_.pitch - requestVel_.pitch;
	velError_.yaw = currentVel_.yaw - requestVel_.yaw;

	// Update the velocity controllers, clamping output forces +/- 100.
	// Only set forces if controller is enabled, otherwise leave zero.
	
	if (axisEnabled_.x & !axisEnabledVel_.x) requestForce_.x = osl_core::limitPlusMinus(velPID_.x.updatePid(velError_.x, deltaSec), 100);
	if (axisEnabled_.y & !axisEnabledVel_.y) requestForce_.y = osl_core::limitPlusMinus(velPID_.y.updatePid(velError_.y, deltaSec), 100);
	if (axisEnabled_.z & !axisEnabledVel_.z) requestForce_.z = osl_core::limitPlusMinus(velPID_.z.updatePid(velError_.z, deltaSec), 100);
	if (axisEnabled_.pitch & !axisEnabledVel_.pitch) requestForce_.pitch = osl_core::limitPlusMinus(velPID_.pitch.updatePid(velError_.pitch, deltaSec), 100);
	if (axisEnabled_.yaw & !axisEnabledVel_.yaw) requestForce_.yaw = osl_core::limitPlusMinus(velPID_.yaw.updatePid(velError_.yaw, deltaSec), 100);
	//PRINTLN_INFO(" POSITION CONTROL : requestForce_.x: "<<requestForce_.x << "\trequestForce_.y: "<<requestForce_.y << "\trequestForce_.z: "<<requestForce_.z << "\trequestForce_.pitch: "<<requestForce_.pitch << "\trequestForce_.yaw: "<<requestForce_.yaw);
}


// ---------------------------------------------------------------------------
void Pilot::updateVelocity(double deltaSec)
{
	if (!initialised_) {
		PRINTLN_ERR("Pilot::update velocity called before init!");
		return;
	}
	// Check altitude is not too low. Cut speed to zeros in the z axis if this is the case.
	if (currentPose_.a <= minAltitude_ && requestVel_.z > 0)
	{
		PRINTLN_INFO("clamping positive z velocity req " << requestVel_.z << " to zero when below min altitude.");
		requestVel_.z = 0.0;
	}
	// Calculate velocity errors.
	velError_.x = currentVel_.x - requestVelMode_.x;
	velError_.y = currentVel_.y - requestVelMode_.y;
	velError_.z = currentVel_.z - requestVelMode_.z;
	velError_.pitch = currentVel_.pitch - requestVelMode_.pitch;
	velError_.yaw = currentVel_.yaw - requestVelMode_.yaw;
	// Update the velocity controllers, clamping output forces +/- 100.
	// Only set forces if controller is enabled, otherwise leave zero.
	if (axisEnabledVel_.x) requestForce_.x = osl_core::limitPlusMinus(velPID_.x.updatePid(velError_.x, deltaSec), 100);
	if (axisEnabledVel_.y) requestForce_.y = osl_core::limitPlusMinus(velPID_.y.updatePid(velError_.y, deltaSec), 100);
	if (axisEnabledVel_.z) requestForce_.z = osl_core::limitPlusMinus(velPID_.z.updatePid(velError_.z, deltaSec), 100);
	if (axisEnabledVel_.pitch) requestForce_.pitch = osl_core::limitPlusMinus(velPID_.pitch.updatePid(velError_.pitch, deltaSec), 100);
	if (axisEnabledVel_.yaw) requestForce_.yaw = osl_core::limitPlusMinus(velPID_.yaw.updatePid(velError_.yaw, deltaSec), 100);
	//PRINTLN_INFO(" VELOCITY CONTROL : requestForce_.x: "<<requestForce_.x << "\trequestForce_.y: "<<requestForce_.y << "\trequestForce_.z: "<<requestForce_.z << "\trequestForce_.pitch: "<<requestForce_.pitch << "\trequestForce_.yaw: "<<requestForce_.yaw);
}

// ---------------------------------------------------------------------------
bool Pilot::inPosition()
{
	return inPositionX() && inPositionY() && inPositionZ() && inPositionPitch() && inPositionYaw();
}

// ---------------------------------------------------------------------------
void Pilot::getAxisInPosition(bool &x, bool &y, bool &z, bool &pitch, bool &yaw)
{
	x = inPositionX();
	y = inPositionY();
	z = inPositionZ();
	pitch = inPositionPitch();
	yaw = inPositionYaw();
}

// ---------------------------------------------------------------------------
const BodyFrameNumeric<double>& Pilot::getPoseError()
{
	return poseError_;
}

// ---------------------------------------------------------------------------
void Pilot::getCurrentWaypoint(std::string& requester, uint32_t& id, uint32_t& priority)
{
	requester = currentWaypoint_.requester;
	id = currentWaypoint_.id;
	priority = currentWaypoint_.priority;
}

// ---------------------------------------------------------------------------
void Pilot::getAxisEnables(bool &x, bool &y, bool &z, bool &pitch, bool &yaw)
{
	x = axisEnabled_.x;
	y = axisEnabled_.y;
	z = axisEnabled_.z;
	pitch = axisEnabled_.pitch;
	yaw = axisEnabled_.yaw;
}

// ---------------------------------------------------------------------------
void Pilot::getRequestedAxisForces(double &x, double &y, double &z, double &pitch, double &yaw)
{
	x = requestForce_.x;
	y = requestForce_.y;
	z = requestForce_.z;
	pitch = requestForce_.pitch;
	yaw = requestForce_.yaw;
}

void Pilot::getRequestedAxisVelocities(double &x, double &y, double &z, double &pitch, double &yaw)
{
	x = requestVel_.x;
	y = requestVel_.y;
	z = requestVel_.z;
	pitch = requestVel_.pitch;
	yaw = requestVel_.yaw;
}


}

